#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include <string.h>
#include "slave.h"
#include "reg.h"
#include "agile_modbus.h"

#define TAG "slave"
#define SLAVE_DATA_LEN 1024

extern const reg_map_t reg_map_head;
extern const reg_obj reg_obj_list;

static QueueHandle_t salveQueue = NULL;

typedef struct{
    uint8_t* data;
    uint16_t len;
}st_send_data;

typedef struct{
    uint8_t* data;
    uint16_t len;
    void (*send)(uint8_t*, uint16_t);
}st_slave_data;

//从机解析Modbus的03功能码
static int modbus_analyze_03_api(void *data,uint16_t *start_reg,uint16_t *len,const reg_map_t* reg_map)
{
    if((data == NULL) || (start_reg == NULL) || (len == NULL))
    {
        return -1;
    }
    *start_reg = ((uint8_t*)data)[2] << 8 | ((uint8_t*)data)[3];
    *len = ((uint8_t*)data)[4] << 8 | ((uint8_t*)data)[5];
    if((*start_reg < reg_map->start_reg) || 
        (*start_reg + *len > reg_map->start_reg + reg_map->len))
    {
        return 1;
    }
    return 0;
}

static int modbus_response_03_api(void *data,uint16_t len,int32_t sts,uint16_t start_reg,uint16_t resp_len,const reg_map_t* reg_map)
{
    if((data == NULL) || (reg_map == NULL))
    {
        return -1;
    }
    uint8_t* resp_data = (uint8_t*)data;
    if(sts == 0)
    {
        if((start_reg < reg_map->start_reg) || 
        (start_reg + resp_len > reg_map->start_reg + reg_map->len))
        {
            return -1;
        }
        uint16_t resp_data_len = 5 + 2 * resp_len;
        if(resp_data_len > len)
        {
            return -1;
        }
        uint8_t byte_count = 2 * resp_len;
        resp_data[0] = reg_map->addr;
        resp_data[1] = 0x03;
        resp_data[2] = byte_count;
        for(uint16_t i = 0; i < resp_len; i++)
        {
            uint16_t reg_addr = start_reg + i;
            //uint16_t reg_val = *(uint16_t*)(reg_map->reg_data + (reg_addr - reg_map->start_reg) * 2);
            uint16_t reg_val = reg_hold_read(reg_addr);
            resp_data[3 + 2 * i] = reg_val >> 8;
            resp_data[4 + 2 * i] = reg_val & 0xff;
        }
        //计算CRC
        uint16_t crc = agile_modbus_crc16(resp_data, resp_data_len - 2);
        resp_data[resp_data_len - 2] = crc >> 8;
        resp_data[resp_data_len - 1] = crc & 0xff;
        return resp_data_len;
    }
    else
    {
        if(5 > len)
        {
            return -1;
        }
        resp_data[0] = reg_map->addr;
        resp_data[1] = 0x03 + 0x80;
        resp_data[2] = sts;
        uint16_t crc = agile_modbus_crc16(resp_data, 3);
        resp_data[3] = crc >> 8;
        resp_data[4] = crc & 0xff;
        return 5;
    }
}

static int modbus_analyze_10_api(void *data,uint16_t *start_reg,uint16_t *len,const reg_map_t* reg_map)
{
    if(data == NULL)
    {
        return -1;
    }
    *start_reg = ((uint8_t*)data)[2] << 8 | ((uint8_t*)data)[3];
    *len = ((uint8_t*)data)[4] << 8 | ((uint8_t*)data)[5];
    if((*start_reg < reg_map->start_reg) || 
    (*start_reg + *len > reg_map->start_reg + reg_map->len))
    {
        return 1;
    }
    //赋值处理
    for(uint16_t i = 0; i < *len; i++)
    {
        uint16_t reg_addr = *start_reg + i;
        uint16_t reg_val = ((uint8_t*)data)[7 + 2 * i] << 8 | ((uint8_t*)data)[8 + 2 * i];
        reg_hold_write(reg_addr, reg_val);
    }
    //传送修改信息
    reg_hold_event(*start_reg, *len);
    return 0;
}

static int modbus_response_10_api(void *data,uint16_t len,int32_t sts,uint16_t start_reg,uint16_t resp_len,const reg_map_t* reg_map)
{
    if((data == NULL) || (reg_map == NULL))
    {
        return -1;
    }
    uint8_t* resp_data = (uint8_t*)data;
    if(sts == 0)
    {
        if(8 > len)
        {
            return -1;
        }
        resp_data[0] = reg_map->addr;
        resp_data[1] = 0x10;
        resp_data[2] = start_reg >> 8;
        resp_data[3] = start_reg & 0xff;
        resp_data[4] = resp_len >> 8;
        resp_data[5] = resp_len & 0xff;
        //计算CRC
        uint16_t crc = agile_modbus_crc16(resp_data, 6);
        resp_data[6] = crc >> 8;
        resp_data[7] = crc & 0xff;
        return 8;
    }
    else
    {
        if(5 > len)
        {
            return -1;
        }
        resp_data[0] = reg_map->addr;
        resp_data[1] = 0x10 + 0x80;
        resp_data[2] = sts;
        uint16_t crc = agile_modbus_crc16(resp_data, 3);
        resp_data[3] = crc >> 8;
        resp_data[4] = crc & 0xff;
        return 5;
    }
}

st_send_data slave_get_send_data(uint8_t* data, uint16_t len)
{
    ESP_LOGE(TAG, "slave data analyze");
    if((len == 0) || (len > SLAVE_DATA_LEN) || (data == NULL))
    {
        ESP_LOGE(TAG, "slave data is valid");
        return (st_send_data){NULL, 0};
    }
    int crc = agile_modbus_crc16(data, len);
    if(crc != 0)
    {
        ESP_LOGE(TAG, "slave data crc is invalid");
        return (st_send_data){NULL, 0};
    }
    uint8_t addr = data[0];
    if(addr!= 0x01)
    {
        ESP_LOGE(TAG, "slave data addr is invalid");
        return (st_send_data){NULL, 0};
    }
    uint8_t fun = data[1];
    if((fun != 0x03) && (fun != 0x10))
    {
        ESP_LOGE(TAG, "slave data fun is invalid");
        return (st_send_data){NULL, 0};
    }
    if(fun == 0x03)
    {
        uint16_t start_reg;
        uint16_t len;
        int sts = modbus_analyze_03_api(data, &start_reg, &len, &reg_map_head);
        if(sts >= 0)
        {
            uint8_t* data_temp = (uint8_t*)pvPortMalloc(len * 2 + 5);
            if(data_temp == NULL)
            {
                ESP_LOGE(TAG, "slave data malloc failed");
                return (st_send_data){NULL, 0};
            }
            else
            {
                uint16_t send_len = modbus_response_03_api(data_temp, len * 2 + 5, sts,start_reg, len, &reg_map_head);
                if(send_len > 0)
                {
                    ESP_LOGE(TAG, "slave data analyze success");
                    return (st_send_data){data_temp, send_len};
                }
                else
                {
                    ESP_LOGE(TAG, "slave data analyze failed");
                    vPortFree(data_temp);
                    return (st_send_data){NULL, 0};
                }
            }
        }
        else
        {
            ESP_LOGE(TAG, "slave data analyze failed");
            return (st_send_data){NULL, 0};
        }
    }
    else if(fun == 0x10)
    {
        uint16_t start_reg;
        uint16_t len;
        int sts = modbus_analyze_10_api(data, &start_reg, &len, &reg_map_head);
        if(sts >= 0)
        {
            uint8_t* data_temp = (uint8_t*)pvPortMalloc(8);
            if(data_temp == NULL)
            {
                ESP_LOGE(TAG, "slave data malloc failed");
                return (st_send_data){NULL, 0};
            }
            else
            {
                uint16_t send_len = modbus_response_10_api(data_temp, 8, sts, start_reg, len, &reg_map_head);
                if(send_len > 0)
                {
                    ESP_LOGE(TAG, "slave data analyze success");
                    return (st_send_data){data_temp, send_len};
                }
                else
                {
                    ESP_LOGE(TAG, "slave data analyze failed");
                    vPortFree(data_temp);
                    return (st_send_data){NULL, 0};
                }
            }
        }
        else
        {
            return (st_send_data){NULL, 0};
        }
    }
    else
    {
        return (st_send_data){NULL, 0};
    }
}

void slave_send_data(uint8_t* data, uint16_t len, void (*send)(uint8_t*, uint16_t))
{
    st_slave_data send_data;
    if((len > 0) && (len <= SLAVE_DATA_LEN) && (data!= NULL))
    {
        send_data.data = (uint8_t*)pvPortMalloc(len);
        if(send_data.data == NULL)
        {
            ESP_LOGE(TAG, "slave data malloc failed");
            return;
        }
        send_data.len = len;
        send_data.send = send;
        memcpy(send_data.data, data, len);
        if(xQueueSend(salveQueue, &send_data, 1000 / portTICK_PERIOD_MS) != pdTRUE)
        {
            ESP_LOGE(TAG, "slave send data failed");
        }
    }
    else
    {
        ESP_LOGE(TAG, "slave data or len is invalid");
    }
}

static void slave_task(void *pvParameters)
{
    salveQueue = xQueueCreate(4, sizeof(st_slave_data));
    uint8_t* data = (uint8_t*)pvPortMalloc(SLAVE_DATA_LEN);
    if((salveQueue == NULL) || (data == NULL))
    {
        ESP_LOGE(TAG, "salve Queue or data create failed");
        return;
    }
    st_slave_data slave_data;
    st_send_data send_data;
    while (1) 
    {
        if(xQueueReceive(salveQueue, &slave_data, portMAX_DELAY) == pdTRUE)
        {
            //do something with slave_data.data and slave_data.len
            if((slave_data.len > 0) && 
                (slave_data.len <= SLAVE_DATA_LEN) && 
                (slave_data.data!= NULL))   
            {
                //do something with slave_data.data and slave_data.len
                memcpy(data, slave_data.data, slave_data.len);
                vPortFree(slave_data.data);
                //解析数据
                ESP_LOGE(TAG, "data id:%d, len:%d", data[0], slave_data.len);
                send_data = slave_get_send_data(data, slave_data.len);
                //是否回复数据
                if((slave_data.send != NULL) && (send_data.data != NULL))
                {
                    ESP_LOGE(TAG, "slave data send is success");
                    slave_data.send(send_data.data, send_data.len);
                }
                else
                {
                    ESP_LOGE(TAG, "slave data send is NULL");
                }
                //释放资源
                if(send_data.data != NULL)
                {
                    vPortFree(send_data.data);
                }
            }
            else
            {
                ESP_LOGE(TAG, "slave data or len is invalid");
                vPortFree(slave_data.data);
            }
        }
    }
    //资源释放
    vQueueDelete(salveQueue);
    salveQueue = NULL;
    vPortFree(data);
    data = NULL;
    vTaskDelete(NULL);
}

void slave_app_main(void)
{
    reg_hold_init();
    //创建任务
    xTaskCreate(slave_task, "slave_task", 4096, NULL, 1, NULL);
}

